﻿using MathNet.Numerics;
using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Double;

class Program
{
    static void PolyFitTest()
    {
          // 示例数据
        double[] xData = { 1, 2, 3, 4, 5 };
        double[] yData = { 1.1, 4.8, 9.1, 16.2, 25.3 };

        // 二次多项式拟合
        var coefficients = Fit.Polynomial(xData, yData, 2);
        Console.WriteLine($"拟合方程：y = {coefficients[2]:F2}x^2 + {coefficients[1]:F2}x + {coefficients[0]:F2}");
    }
    static void MatrixTest()
    { 
        var matrixA = Matrix<double>.Build.DenseOfArray(new double[,] { {1, 2}, {3, 4} });
        var matrixB = Matrix<double>.Build.DenseOfArray(new double[,] { {5, 6}, {7, 8} });
        var productMatrix = matrixA * matrixB;
        Console.WriteLine(productMatrix);
    }
    static void MatrixTest2()
    { 
        List<double> h_matrix_factors =new List<double>() { 1,2,3,4};
        var h_matrix_id = DenseMatrix.OfColumnMajor(2,2, h_matrix_factors);
        Console.WriteLine(h_matrix_id);
        Console.WriteLine("-------------------------");
        var h_mat_transpose = h_matrix_id.Transpose();
         Console.WriteLine(h_mat_transpose);
    }

    static void MatrixTest3()
    {
        // 创建一个 3x3 的矩阵
        var matrix = Matrix<double>.Build.DenseOfArray(new double[,] {
            {1, 0, 0,0 },
            {0, 1, 0,0 },
            {1, 0, 1,0 },
            {0, 0, 0,1 }
        });

        // 显示原矩阵
        Console.WriteLine("Original Matrix:");
        Console.WriteLine(matrix);

        // 检查矩阵是否可逆
        if (matrix.Determinant() == 0)
        {
            Console.WriteLine("The matrix is singular and cannot be inverted.");
        }
        else
        {
            // 求矩阵的逆
            var inverse = matrix.Inverse();

            // 显示逆矩阵
            Console.WriteLine("Inverse Matrix:");
            Console.WriteLine(inverse);
        }
    }

    static void MatrixTest4()
    { 
        var A = Matrix<double>.Build.DenseOfArray(new double[,] { {2, 1, -1}, {3, 3, 3}, {1, -1, 2} });
        var b = Vector<double>.Build.DenseOfArray(new double[] { 1, 9, 6 });
        var lu = A.LU();
        var x = lu.Solve(b);
        Console.WriteLine(x);

        }
    static Matrix<double> BuildDesignMatrix(double[] xData)
    {
        int n = xData.Length;
        Matrix<double> A = Matrix<double>.Build.Dense(n, 2);
        for (int i = 0; i < n; i++)
        {
            A[i, 0] = xData[i];
            A[i, 1] = 1.0;
        }
        return A;
    }
    static void MatrixTest5()
    {
        // 示例数据
        double[] xData = { 1, 2, 3, 4, 5 };
        double[] yData = { 2.2, 4.1, 6.0, 8.2, 10.3 };

        // 构建设计矩阵 A
        Matrix<double> A = BuildDesignMatrix(xData);
        Vector<double> y = Vector<double>.Build.Dense(yData);

        // 计算 A^T * A
        Matrix<double>AtA = A.Transpose() * A;

        // 计算 A^T * y
        Vector<double>Aty = A.Transpose() * y;

        // 求解 (A^T * A) * x = A^T * y
        Vector<double> x = AtA.Solve(Aty);

        // 提取拟合参数
        double slope = x[0];  // 斜率
        double intercept = x[1];  // 截距

        // 显示拟合结果
        Console.WriteLine($"拟合方程：y = {slope:F4}x + {intercept:F4}");

        // 计算拟合值
        double[] yFit = new double[xData.Length];
        for (int i = 0; i < xData.Length; i++)
        {
            yFit[i] = slope * xData[i] + intercept;
        }

        // 显示拟合值
        Console.WriteLine("\n拟合结果：");
        for (int i = 0; i < xData.Length; i++)
        {
            Console.WriteLine($"{xData[i]}: {yData[i]:F2} (实际值) -> {yFit[i]:F2} (拟合值)");
        }
    }


    static Matrix<double> GenerateRandomAffineMatrix()
    {
        // 生成随机的旋转角度（以弧度为单位）
        Random rand = new Random();
        double theta = rand.NextDouble() * 2 * Math.PI;

        // 生成随机的缩放因子
        double scale_x = 0.5 + rand.NextDouble() * 1.5;
        double scale_y = 0.5 + rand.NextDouble() * 1.5;

        // 生成随机的剪切因子
        double shear_x = -0.5 + rand.NextDouble() * 1.0;
        double shear_y = -0.5 + rand.NextDouble() * 1.0;

        // 生成随机的平移量
        double translate_x = -100 + rand.NextDouble() * 200;
        double translate_y = -100 + rand.NextDouble() * 200;

        // 构建仿射变换矩阵
        var affineMatrix = Matrix<double>.Build.DenseOfArray(new double[,] {
            { scale_x * Math.Cos(theta), scale_y * Math.Sin(theta + shear_y), translate_x },
            { -scale_x * Math.Sin(theta + shear_x), scale_y * Math.Cos(theta), translate_y },
            { 0, 0, 1 }
        });

        return affineMatrix;
    }
    static Vector<double> ApplyAffineTransformation(Matrix<double> matrix, Vector<double> point)
    {
        // 确保点是齐次坐标
        if (point.Count != 3)
        {
            throw new ArgumentException("Point must be in homogeneous coordinates (3 elements).");
        }

        // 应用仿射变换
        var transformedPoint = matrix * point;

        // 归一化齐次坐标
        if (transformedPoint[2] != 0)
        {
            transformedPoint = transformedPoint / transformedPoint[2];
        }

        return transformedPoint;
    }
    static void MatrixTest6()
    {
        // 生成一个随机的仿射矩阵
        var randomAffineMatrix = GenerateRandomAffineMatrix();

        // 显示仿射矩阵
        Console.WriteLine("随机生成的仿射矩阵:");
        Console.WriteLine(randomAffineMatrix);

        
        // 定义一个点进行变换
        var point = Vector<double>.Build.Dense(new double[] { 1.0, 2.0, 1.0 });

        // 应用仿射变换
        var transformedPoint = ApplyAffineTransformation(randomAffineMatrix, point);

        // 显示变换后的点
        Console.WriteLine("\n变换前的点: (1.0, 2.0)");
        Console.WriteLine($"变换后的点: ({transformedPoint[0]:F4}, {transformedPoint[1]:F4})");
    }


    static void MatrixTest7()
    {
        // 原始点和变换后的点
        var originalPoints = new double[3,3];
        var transformedPoints = new  double[3,3];

        // 定义原始点和变换后的点（示例数据）
        originalPoints[0, 0] = 0; originalPoints[0, 1] = 0; originalPoints[0, 2] = 1;
        originalPoints[1, 0] = 1; originalPoints[1, 1] = 0; originalPoints[1, 2] = 1;
        originalPoints[2, 0] = 0; originalPoints[2, 1] = 1; originalPoints[2, 2] = 1;

        transformedPoints[0, 0] = 2; transformedPoints[0, 1] = 3; transformedPoints[0, 2] = 1;
        transformedPoints[1, 0] = 4; transformedPoints[1, 1] = 3; transformedPoints[1, 2] = 1;
        transformedPoints[2, 0] = 2; transformedPoints[2, 1] = 5; transformedPoints[2, 2] = 1;

        // 计算仿射变换矩阵
        var affineMatrix = CalculateAffineMatrix(originalPoints, transformedPoints);

        // 显示仿射矩阵
        Console.WriteLine("计算得到的仿射矩阵:");
        Console.WriteLine(affineMatrix);
    }

    static Matrix<double> CalculateAffineMatrix(double[,] originalPoints, double[,] transformedPoints)
    {
        // 构建方程组 A * X = B
        var A = Matrix<double>.Build.Dense(6, 6);
        var B = Vector<double>.Build.Dense(6);

        for (int i = 0; i < 3; i++)
        {
            int row = i * 2;
            A[row, 0] = originalPoints[i, 0];
            A[row, 1] = originalPoints[i, 1];
            A[row, 2] = 1;
            A[row, 3] = 0;
            A[row, 4] = 0;
            A[row, 5] = 0;
            B[row] = transformedPoints[i, 0];

            row = i * 2 + 1;
            A[row, 0] = 0;
            A[row, 1] = 0;
            A[row, 2] = 0;
            A[row, 3] = originalPoints[i, 0];
            A[row, 4] = originalPoints[i, 1];
            A[row, 5] = 1;
            B[row] = transformedPoints[i, 1];
        }

        // 解方程组
        var X = A.QR().Solve(B);

        // 构建仿射矩阵
        var affineMatrix = Matrix<double>.Build.Dense(3, 3);
        affineMatrix[0, 0] = X[0];
        affineMatrix[0, 1] = X[1];
        affineMatrix[0, 2] = X[2];
        affineMatrix[1, 0] = X[3];
        affineMatrix[1, 1] = X[4];
        affineMatrix[1, 2] = X[5];
        affineMatrix[2, 0] = 0;
        affineMatrix[2, 1] = 0;
        affineMatrix[2, 2] = 1;

        return affineMatrix;
    }
    static void MatrixTest8()
    { 
        List<double > h_matrix_factors = new List<double>{ };
        h_matrix_factors.AddRange(new double[]{1, 0, 0.3, 0 } );
        h_matrix_factors.AddRange(new double[]{0, 1, 0,  0.2 } );
        h_matrix_factors.AddRange(new double[]{0, -0.3, 1, 0 } );
        h_matrix_factors.AddRange(new double[]{0, -0.2, 0, 1 } );
        var h_matrix_id = DenseMatrix.OfColumnMajor(4, 4, h_matrix_factors);
        var h_matrix_inv = h_matrix_id.Inverse();
        var obs_vec = Vector<double>.Build.Dense(new double[] { 3.2, - 0.3, - 1.6, - 0.8 });
        var motor_vec = h_matrix_inv*obs_vec;
        var obs_vec2 = h_matrix_id*motor_vec;
        Console.WriteLine(motor_vec);
        Console.WriteLine(obs_vec2);
    }
    static void Main()
    {
      MatrixTest8();
    }
}